#include <hit_ctrl/hit_ctrl.hpp>
#include <fstream>
#include <iostream>
 

base_interface::msg::CvData CV_msg;

std::ofstream outfilelearning;         //文件 记录数据
std::ofstream outfilelearning_angle;

std::ofstream outfilelearning_q;         //读取q 用来使用的
std::ofstream outfilelearning_q_read;   //看数据对不对生成Q[][]=;


int num1=0;  
int Qlearning_train_flag = 1;
int Qlearning_use_flag = 0;
int get_cv_predict_flag = 1;          //倾角的标志位  用于只改一次倾角
int hit_ball_flag  = 1;               //击球的标志位  用于只击一次球
int bump_finish_flag = 0;             //击球完成flag
int receive_bump_finish_flag=0;       //接收击球完成flag
float max_height = 0;                 //记录击球过程中的最高点
float last_max_height = 0;            //记录上一次颠球过程中的最高点
float motor_pitch = 0;
float motor_roll = 0;
auto hit = base_interface::msg::Hit();

//////---------------real-------------------
HitCtrl::HitCtrl():Node("Hit_Ctrl_Node")
{
  printf("Hit_Ctrl_Node节点创建\n");
  int i,j; //for循环变量
   // 创建输出文件
    // 获取当前的系统时间
    time_t currentTime;
    time(&currentTime);
    //	格林尼治标准时间+8个小时
    currentTime = currentTime + 8 * 3600; 
    // 将调整后的时间转换为 tm 结构，以便按部分（如年、月、日等）访问  
    tm *t = gmtime(&currentTime);	  
    // std::string filename = "data/learning/outfilelearning" +std::to_string(t->tm_mon + 1) + "-" +std::to_string(t->tm_mday) + "-" + std::to_string(t->tm_hour) + "-" + std::to_string(t->tm_min) + ".txt";
    // // 打开第一个输出文件以供写入
    // outfilelearning.open(filename.c_str());
    std::string filename_time = "data/learning/outfile_angle" + to_string(t->tm_mon + 1) + "-" +to_string(t->tm_mday) + "-" + to_string(t->tm_hour) + "-" + to_string(t->tm_min) + ".txt";
    outfilelearning_angle.open(filename_time .c_str());

   std::string filename_q_read = "data/learning/read/outfile_q_read.txt";
   outfilelearning_q_read.open(filename_q_read .c_str(), ios::out|ios::app);   //人看的q表文件

   std::string filename_q = "data/learning/used/outfile_q.txt";
    outfilelearning_q.open(filename_q .c_str(), ios::out|ios::app);      //读的q表 参数给程序内的q表
  

  hit_pub = this->create_publisher<base_interface::msg::Hit>("/cmd_hit",10);
  cvdata_receive = this->create_subscription<base_interface::msg::CvData>("Cv_Talker",10 ,std::bind(&HitCtrl::CvMessageCallback,this,std::placeholders::_1));
  mcu_receive = this->create_subscription<base_interface::msg::McuComm>("Slave_Data",10 ,std::bind(&HitCtrl::McuCommCallback,this,std::placeholders::_1) );
  
    //-----------加入读取文件到数组------------------
    std::ifstream inputFile(filename_q); // 使用给定的文件名打开文件进行读取操作
  
    if (inputFile.is_open()) 
       { 
          std::cout << "文件已成功打开！" << std::endl;
          std::string line;
          while (std::getline(inputFile, line)) 
                {   
                    std::istringstream iss(line);
                    int value1,value2,value3;
                    if (iss >> value1 >> value2 >> value3) 
                    {   
                      q[value1][value2]=value3;
                    }
                    else 
                    {
                        std::cerr << "无法读取数据！" << std::endl;
                    }

                } 
          inputFile.close(); // 关闭文件
          for(i=0;i<20;i++)
          {
             for(j=0; j< 20; j++)
               printf("q[%d][%d]=%d\n",i,j,q[i][j]);
          }
       }
//--------------------------------------------------------------------------- 

}

typedef struct
{
    float Setangle;           
    float Actualangle;       
    float integral;
    float err;                
    float err_last;            
    float Kp, Ki, Kd;   
    float Kp1, Ki1, Kd1;  
             
}pidvel_t;

pidvel_t pid_roll,pid_pitch;

pidvel_t pid_vel_y,pid_vel_x;

void PID_init()
{
    pid_roll.integral = 0.0;
    pid_roll.err = 0.0;
    pid_roll.err_last = 0.0;
    pid_roll.Kp = 70.0;
    pid_roll.Ki = 0.4;
    pid_roll.Kd = 10.0;

    pid_pitch.integral = 0.0;
    pid_pitch.err = 0.0;
    pid_pitch.err_last = 0.0;
    pid_pitch.Kp = 80.0;
    pid_pitch.Ki = 0.1;
    pid_pitch.Kd = 10.0;

    pid_vel_x.integral = 0.0;
    pid_vel_x.err = 0.0;
    pid_vel_x.err_last = 0.0;
    pid_vel_x.Kp = 0.05;
    pid_vel_x.Ki = 0;
    pid_vel_x.Kd = 0.2;

    pid_vel_y.integral = 0.0;
    pid_vel_y.err = 0.0;
    pid_vel_y.err_last = 0.0;
    pid_vel_y.Kp = 10;
    pid_vel_y.Ki = 0;
    pid_vel_y.Kd = 0;


}

float PID_compute_roll(float errd)
{
  if(fabs(errd)<0.3)
   {
      pid_roll.integral += errd;
      float incrementSpeed = pid_roll.Kp * errd + pid_roll.Ki * pid_roll.integral + pid_roll.Kd * (errd - pid_roll.err_last);
      pid_roll.err_last = errd;
      return (incrementSpeed);
   }
  else
      return 0;
}

float PID_compute_vel_x(float errd)
{
  if(fabs(errd)<0.3)
   {
      pid_vel_x.integral += errd;
      float incrementSpeed = pid_vel_x.Kp * errd + pid_vel_x.Ki * pid_vel_x.integral + pid_vel_x.Kd * (errd - pid_vel_x.err_last);
      pid_vel_x.err_last = errd;
      return (incrementSpeed);
   }
  else
      return 0;
}


float PID_compute_pitch(float errd)
{
  if(fabs(errd)<0.3)
   {
      pid_pitch.integral += errd;
      float incrementSpeed = pid_pitch.Kp * errd + pid_pitch.Ki * pid_pitch.integral + pid_pitch.Kd * (errd - pid_pitch.err_last);
      pid_pitch.err_last = errd;
      return (incrementSpeed);
    }
  else
      return 0;
}

float PID_compute_vel_y(float errd)
{
  if(fabs(errd)<0.3)
   {
      pid_vel_y.integral += errd;
      float incrementSpeed = pid_vel_y.Kp * errd + pid_vel_y.Ki * pid_vel_y.integral + pid_vel_y.Kd * (errd - pid_vel_y.err_last);
      pid_vel_y.err_last = errd;
      return (incrementSpeed);
   }
  else
      return 0;
}


float parabola_compute_roll(float errd)
{
   if(fabs(errd)<0.5)
   {
    float k = 1.f / 1700.f * errd * 1000.f; //斜率
    return (atan2(k,1.f) / PI * 180.0);
    }
  else
      return 0;
}

float parabola_compute_pitch(float errd)
{
  if(fabs(errd)<0.5)
   {
    float k = 1.f / 1700.f * errd * 1000.f; //斜率
    return atan2(k,1.f) / PI * 180.0;
    }
  else
      return 0;
}

void HitCtrl::CvMessageCallback(const base_interface::msg::CvData::SharedPtr cv_msg)
{

  max_height = max_height > cv_msg->current_z ? max_height : cv_msg->current_z;
  
  Eigen::Vector2f tilt;
  tilt.x() = 0;  tilt.y() = 0; 

  // auto hit = base_interface::msg::Hit();

  //球下降过程中的期望————落点位置
  //---------------------------------------------------------------------------------------------------------------------------------------------
  if ( cv_msg->current_z < 1.2 && (cv_msg->current_speed_z < 0.0) && (get_cv_predict_flag == 1))
  {

          bump_finish_flag = 0;  //颠球完成的flag = 0;
        
          hit.cv_end_x = 0.0 * cv_msg->end_x + 1.0 * cv_msg->current_x;//cv_msg->current_x;//cv_msg->end_x;
          hit.cv_end_y =0.178 + 0.0 * (cv_msg->end_y - 0.178) + 1.0 * (cv_msg->current_y -0.178);//cv_msg->current_y;//cv_msg->end_y;
          
          // hit.cv_end_x = cv_msg->end_x; //cv_msg->current_x; //cv_msg->end_x;
          // hit.cv_end_y = cv_msg->end_y; //cv_msg->current_y; //cv_msg->end_y;
          
          // hit.cv_end_x = 0.2 ;
          // hit.cv_end_y = 0.2;

          outfilelearning_angle<<"hit.cv_end_x:\t"<<hit.cv_end_x<<"\thit.cv_end_y\t"<<hit.cv_end_y<<endl;
          hit.hit_move_flag = 1; 
  }

  //  //底盘实时跟随
  //  hit.cv_end_x = 0.0 * cv_msg->end_x + 1.0 * cv_msg->current_x;//cv_msg->current_x;//cv_msg->end_x;
  //  hit.cv_end_y = 0.178 + 0.0 * (cv_msg->end_y - 0.178) + 1.0 * (cv_msg->current_y -0.178);//cv_msg->current_y;//cv_msg->end_y;
  //  hit.hit_move_flag = 1; 

//---------------------------------------------------------------------------------------------------------------------------------------------

  //计算球下落过程中平台的————倾角
  if ( cv_msg->current_z < 1.2 && (cv_msg->current_speed_z < -0.05) && (get_cv_predict_flag == 1))
  {
      bump_finish_flag = 0;        //颠球完成的flag = 0;

      if (fabs(cv_msg->end_pitch) < 1.0 && fabs(cv_msg->end_yaw) < 1.0){
      hit.target_pitch_motor = 0;
      hit.target_roll_motor = 0;
      printf("11111111\n");
    }
    else{
      tilt = Throw_the_ball_oneself(cv_msg->end_pitch, cv_msg->end_yaw);

      if(0.20 - cv_msg->current_y < 0) motor_pitch -= 0.5;
      else motor_pitch += 0.5;

      if(0 - cv_msg->current_x < 0)  motor_roll -= 0.5;
      else motor_roll += 0.5;

      hit.target_pitch_motor = 0.0;// motor_pitch; //parabola_compute_pitch( 0.20 - cv_msg->current_y );// 0.2 * PID_compute_pitch( 0.19 - cv_msg->current_y );// parabola_compute_pitch( 0.20 - cv_msg->current_y , (max_height - 0.6));//0.0;// tilt.x();//0.3 * PID_compute_pitch( 0.19 - cv_msg->current_y );//0;//parabola_compute_pitch( 0.20 - cv_msg->current_y);//0;//PID_compute_pitch( 0.19 - cv_msg->current_y );// tilt.x();// 0;// PID_compute_pitch( 0.19 - cv_msg->current_y );// tilt.x();//parabola_compute_pitch( 0.20 - cv_msg->current_y , cv_msg->max_z );// tilt.x(); //PID_compute_pitch( 0.19 - cv_msg->current_y ); // - PID_compute_vel_y(0.0 - cv_msg->current_speed_y); //parabola_compute_pitch( 0.20 - cv_msg->current_y );//PID_compute_pitch( 0.2 - cv_msg->current_y ) + PID_compute_vel_y(0.0 - cv_msg->current_speed_y);//parabola_compute_pitch( 0.20 - cv_msg->current_y );// PID_compute_pitch( 0.2 - cv_msg->current_y ) + PID_compute_vel_y(0.0 - cv_msg->current_speed_y); //parabola_compute_pitch( 0.20 - cv_msg->current_y );//PID_compute_pitch( 0.2 - cv_msg->current_y ); //tilt.x();
      hit.target_pitch_motor = hit.target_pitch_motor ;//+ 1.3 * hit.target_pitch_motor/fabs(hit.target_pitch_motor);
      hit.target_roll_motor  = 0.0;//motor_roll;//parabola_compute_roll( 0 - cv_msg->current_x );    //0.2 * PID_compute_roll(0 - cv_msg->current_x) ;// parabola_compute_roll( 0 - cv_msg->current_x , (max_height - 0.6));//0.0;//tilt.y(); //0.3 * PID_compute_roll(0 - cv_msg->current_x) ;// 0;//parabola_compute_roll( 0 - cv_msg->current_x );//0;//PID_compute_roll(0 - cv_msg->current_x) ;// tilt.y();//0; // PID_compute_roll(0 - cv_msg->current_x) ;//tilt.y();//parabola_compute_roll( 0 - cv_msg->current_x, cv_msg->max_z );//tilt.y();//PID_compute_roll(0 - cv_msg->current_x) ;//parabola_compute_roll( 0 - cv_msg->current_x);//PID_compute_roll(0 - cv_msg->current_x) + PID_compute_vel_x(0.0 - cv_msg->current_speed_x);//parabola_compute_roll( 0 - cv_msg->current_x);// PID_compute_roll(0 - cv_msg->current_x) + PID_compute_vel_x(0.0 - cv_msg->current_speed_x);//parabola_compute_roll( 0 - cv_msg->current_x);//PID_compute_roll(0 - cv_msg->current_x);  //tilt.y();
      hit.target_roll_motor = hit.target_roll_motor ;//+ 1.3 * hit.target_roll_motor/fabs(hit.target_roll_motor);
    }

    if(hit.target_pitch_motor >= 53)  hit.target_pitch_motor = 53;  //限幅
    if(hit.target_pitch_motor <= -8)  hit.target_pitch_motor = -8;
    if(hit.target_roll_motor >= 43)   hit.target_roll_motor =  43;
    if(hit.target_roll_motor <= -43)  hit.target_roll_motor = -43;

    // if(hit.target_pitch_motor >= 10)  hit.target_pitch_motor = 10;  //限幅
    // if(hit.target_pitch_motor <= -8) hit.target_pitch_motor = -8;
    // if(hit.target_roll_motor >= 10)   hit.target_roll_motor = 10;
    // if(hit.target_roll_motor <= -10)  hit.target_roll_motor = -10;

     get_cv_predict_flag = 0;  //倾角flag置零一次过后不再进入
  }

  if(cv_msg->current_z  < 0.6  && fabs(cv_msg->current_speed_z) < 0.05 && bump_finish_flag)
  {
       hit.hit_vel  = 4.0; 
  }


// //加入判断击球时机，单独放在一个判断里——击球——给下位机
  if(cv_msg->current_z  < 0.95  && (cv_msg->current_speed_z < -0.05) && (hit_ball_flag == 1))
  {
      bump_finish_flag = 0;
      num1++;
      number_ball_bounce++;

        if(number_ball_bounce==2 && Qlearning_train_flag)  //第二次颠球 —— 状态初始化
          {
              last_volleyball_highest = max_height * 1000;             //高度单位毫米
              last_volleyball_targetpos_vel = sqrtf32(2.0 * 9.8 * (max_height - 0.6)) ;    //速度方向相反

              state_init_receive = state_init(); //初始化

              if( !state_init_receive )
              {
                number_ball_bounce = 1;
              }
              else
              {
                hit.hit_vel = oumiga;
              }
          }


        if(number_ball_bounce>2 && Qlearning_train_flag)  //训练程序  //从第三次颠球开始训练
          {   
              number_ball_bounce = 3 ;

              volleyball_highest = max_height * 1000;             //高度单位毫米
              volleyball_targetpos_vel = sqrtf32(2.0 * 9.8 * (max_height - 0.6)) ;    //速度方向相反

              Q_learning_train(); 

              if((int)train_point.v!=0)
              {
                      ballVelWish  = train_point.v;
                      anglevelWish = train_point.w;     
                      q_value      = train_point.q;
                      hit.hit_vel  = oumiga;                 //实际的击球速度
                      base_time = this->now();               //时间戳  打印出来的
                      outfilelearning_q_read<<"q["<<ballVelWish<<"]["<<anglevelWish  <<"] = "<<q_value<<" ;"<<"\n"<<std::endl;
                      outfilelearning_q<<ballVelWish<<"\t"<<anglevelWish<<"\t"<<q_value<<"\n"<<std::endl;
              }
              else
              {
                      hit.hit_vel = 4.0;
              }
          } 


        if(Qlearning_use_flag)   //使用程序
        {
               Q_learning_use();
               hit.hit_vel  = oumiga;
        }

    hit.hit_vel  = 4.0;
    hit.hit_mcu_flag =1; //给主控发送颠球信号flag
    hit_ball_flag =0;    //击球flag置零一次过后不再进入
  }
  
   
   hit_pub ->publish(hit);


   std::cout<<"**********************"<<hit.hit_vel<<std::endl;
   std::cout<<"**********************"<<bump_finish_flag<<std::endl;

   

//---------------------------------------------------------------------------------------------------------------------------------------

  // //球上升过程，且高度大于0.6m时，重置
  if ((cv_msg->current_speed_z > 0.05 ) && (cv_msg->current_z >= 0.6)){
         get_cv_predict_flag = 1;           //倾角flag
         hit_ball_flag=1;                   //击球flag
         last_max_height = max_height;      //将这一次的最高值赋值给上一次。
         max_height = 0.0;                  //初始化最高点的值
         volleyball_targetpos_vel = 0.0;    //初始化落点z方向速度的值   
        //  hit.target_pitch_motor = 0;        //倾角归零
        //  hit.target_roll_motor = 0;         //俯仰归零
  }

  // if(bump_finish_flag)
  // {
  //        
  // }
 

  printf("cv_pitch:%.2f,cv_yaw:%.2f   tilt_pitch:%.2f,tilt_roll:%.2f,hit.target_pitch_motor:%.2f,hit.target_roll_motor:%.2f \n",cv_msg->end_pitch,cv_msg->end_yaw,tilt.x(),tilt.y(),hit.target_pitch_motor,hit.target_roll_motor);
  outfilelearning_angle <<"cv_pitch:\t"
                        <<cv_msg->end_pitch
                        <<"\tcv_yaw:\t"
                        <<cv_msg->end_yaw  
                        <<"\t delta_time:\t"
                        <<cv_msg->delta_time 
                        <<"\t hit_vel:\t"
                        <<hit.hit_vel<<"\t"
                        <<"\t bump_finish_flag:"<<"\t"
                        <<bump_finish_flag
                        <<"\t num1:\t"
                        <<num1<<"\t"
                        <<"\t current_speed_z:\t"
                        <<cv_msg->current_speed_z<<"\t"
                        <<"current_z:\t"
                        <<cv_msg->current_z<<"\t"
                        <<"end_speed_z:\t"
                        <<cv_msg->end_speed_z<<"\t"
                        // <<"tilt_pitch:\t"
                        // <<tilt.x()
                        // <<"\t tilt_roll:\t"
                        // <<tilt.y()
                        <<"\t hit.target_pitch_motor:\t" 
                        <<hit.target_pitch_motor
                        <<"\t hit.target_roll_motor:\t" 
                        <<hit.target_roll_motor
                        <<"\t base_time:\t" 
                        <<base_time.nanoseconds()
                        <<"\t max_height:\t" 
                        <<max_height
                        <<"\t 0.6m_vel:\t" 
                        <<volleyball_targetpos_vel
                        <<std::endl; 

    
   hit.hit_mcu_flag = 0;    //给主控发送一次过后置零
   hit.hit_vel  = 0;        //颠球策略速度置零
   hit.hit_move_flag = 0;   //移动策略位置flag置零

}


void HitCtrl::McuCommCallback(const base_interface::msg::McuComm::SharedPtr mcu_msg){

 if( receive_bump_finish_flag == 0 )
 {
    bump_finish_flag =  (int)mcu_msg->num[12];
 }
  
  // start_bump_flag  =  (int)mcu_msg->num[11];
  // mcu_msg.num[12];
  // cout<<"bump_finish_flag   "<<bump_finish_flag<<endl;

}

/*
 *自颠球，由球的俯仰角和偏航角得到机构的俯仰角和横滚角
*/
Eigen::Vector2f Throw_the_ball_oneself(float cv_end_pitch,float cv_end_yaw)
{
  //角度制  -----得看下 视觉那边用的是角度还是弧度    是角度
  
  float ball_pitch = cv_end_pitch;//*180.0/PI;       //80.534;//球的俯仰角 // float ball_pitch = 43.839;//球的俯仰角
 
  float ball_yaw = cv_end_yaw;//*180.0/PI;           //6.4349;//球的偏航角     // float ball_yaw = 39.806;//球的偏航角

  float theta; //法线与Z轴的夹角
  theta = 0.5*(90-ball_pitch);

  // float pitch;//机构俯仰角
  // float roll;//机构的横滚角
  
  Eigen::Vector2f tilt;


  theta = theta/180*PI;   //弧度
  ball_pitch = ball_pitch/180*PI;
  ball_yaw = ball_yaw/180*PI;

  //电机那边接受的是角度
  tilt.x() = atan(sin(theta)*cos(ball_yaw)/cos(theta))/PI*180;      //pitch
//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//横滚改为 从车屁股看，左为负，右为正
  tilt.y() = - atan(sin(theta)*sin(ball_yaw)/cos(theta))/PI*180;      //roll

  policy_ball.pitch = tilt.x();
  policy_ball.roll  = tilt.y();
  if(state_ball.yaw != 0)
  policy_ball.yaw = state_ball.yaw;
  
 // cout<<"球的俯仰角和偏航角:"<< ball_pitch/PI*180<<","<<ball_yaw/PI*180<<"  机构的俯仰角和横滚角:"<< tilt.x()<<"  "<<tilt.y()<<endl;
  return tilt;
}

// //----------------------test-------------------------------------------
// HitCtrl::HitCtrl():Node("Hit_Ctrl_Node")
// {
//   printf("Hit_Ctrl_Node节点创建\n");
//   hit_pub = this->create_publisher<base_interface::msg::Hit>("/cmd_hit",10);
//   //----------test---------------------------------             
//  timer_ = this->create_wall_timer(500ms, std::bind(&HitCtrl::CvMessageCallback, this));    
//   //-----------------------------------------------
// }
// void HitCtrl::CvMessageCallback()
// {

//   Eigen::Vector2f tilt;
//    auto hit = base_interface::msg::Hit();
//   float cv_pitch,cv_yaw; 
// //----------test---------------------------------
//     cv_pitch=45; 
//     cv_yaw=180; 

//    tilt=Throw_the_ball_oneself(cv_pitch ,cv_yaw); 
//    hit.target_pitch_motor =tilt.x(); 
//    hit.target_roll_motor  =tilt.y(); 
//   //  hit.hit_vel=1;
// //-----------------------------------------------
  
//   if(hit.target_pitch_motor>=53)  hit.target_pitch_motor=53;   
//   if(hit.target_pitch_motor<=-10) hit.target_pitch_motor=-9;   
//   if(hit.target_roll_motor>=45 )  hit.target_roll_motor=44;     
//   if(hit.target_roll_motor<=-45 ) hit.target_roll_motor=-44;   
//   printf("cv_pitch:%.2f,cv_yaw:%.2f    tilt_pitch:%.2f,tilt_roll:%.2f    hit.hit_vel:%.2f \n", cv_pitch ,cv_yaw ,tilt.x(),tilt.y(),hit.hit_vel);

//    hit_pub ->publish(hit);
// }


// Eigen::Vector2f Throw_the_ball_oneself(float cv_end_pitch,float cv_end_yaw)
// {
//   //角度制  -----得看下 视觉那边用的是角度还是弧度    是角度
  
//   float ball_pitch = cv_end_pitch;//*180.0/PI;       //80.534;//球的俯仰角 // float ball_pitch = 43.839;//球的俯仰角
 
//   float ball_yaw = cv_end_yaw;//*180.0/PI;           //6.4349;//球的偏航角     // float ball_yaw = 39.806;//球的偏航角

//   float theta; //法线与Z轴的夹角
//   theta = 0.5*(90-ball_pitch);

//   // float pitch;//机构俯仰角
//   // float roll;//机构的横滚角
  
//   Eigen::Vector2f tilt;


//   theta = theta/180*PI;   //弧度
//   ball_pitch = ball_pitch/180*PI;
//   ball_yaw = ball_yaw/180*PI;

//   //电机那边接受的是角度
//   tilt.x() = atan(sin(theta)*cos(ball_yaw)/cos(theta))/PI*180;   //pitch
//   tilt.y() = atan(sin(theta)*sin(ball_yaw)/cos(theta))/PI*180;   //roll

//   return tilt;
// }

// //------------------------------------------------------







/**
 * 计算击球平台的中心点相对于原点的位置-----------------------------------------------------------------------------------------------------------------
*/
void The_position_of_the_batting_platform()
{ 
     float L1 = 110.0;
     float L2 = 191.0;
     float L3 = 68.0;
     float h0 = 191.0;

     float theta1 = policy_ball.yaw;   //0.0;
     float theta2 = policy_ball.roll;  //0.0;
     float theta3 = policy_ball.pitch; //0.0;
     float theta4 = 0.0;

     Eigen::MatrixXd T0_1 = Eigen::MatrixXd::Zero( 4,4 );
     Eigen::MatrixXd R0_1 = Eigen::MatrixXd::Zero( 4,4 );
     Eigen::MatrixXd T1_2 = Eigen::MatrixXd::Zero( 4,4 );
     Eigen::MatrixXd R1_2 = Eigen::MatrixXd::Zero( 4,4 );
     Eigen::MatrixXd T2_3 = Eigen::MatrixXd::Zero( 4,4 );
     Eigen::MatrixXd R2_3 = Eigen::MatrixXd::Zero( 4,4 );
     
     Eigen::VectorXd p1_1(4); 
     Eigen::VectorXd p1_0(4);
     Eigen::VectorXd p2_1(4); 
     Eigen::VectorXd p2_0(4); 
     Eigen::VectorXd p3_1(4); 
     Eigen::VectorXd p3_3(4); 
     Eigen::VectorXd v3_1(4);
     Eigen::VectorXd v3_3(4);  
     Eigen::VectorXd p3_0(4);  

     float H = h0+(5.42311682569715E-10)*pow((theta4*180.0/PI),6)+(-1.93651398665508E-07)*pow((theta4*180.0/PI),5)+(0.0000302647117567201)*pow((theta4*180.0/PI),4)+(-0.00264004963371833)*pow((theta4*180.0/PI),3)+(0.083007456266404)*pow((theta4*180.0/PI),2)+(2.42443501456533)*(theta4*180.0/PI)+1.34324368923143;

     T0_1 <<   1,        0,     0 ,       0,
               0,        1,     0,        L1,
               0,        0,     1,        0,
               0,        0,     0,        1;
     
     R0_1 <<   cos(theta1),                   -sin(theta1),            0 ,          0,
               sin(theta1),                    cos(theta1),            0,           0,
                     0,                              0,                1,           0,
                     0,                              0,                0,           1;
     
     T1_2 <<   1,        0,        0,        0,
               0,        0,        1,        0,
               0,       -1,        0,        L2,
               0,        0,        0,        1;
                
          
     R1_2 <<    cos(theta2),          -sin(theta2),         0,           0,
                sin(theta2),           cos(theta2),         0,           0,
                     0,                    0,               1,           0,
                     0,                    0,               0,           1;

     T2_3<<    0,        0,     -1 ,        0,
               0,        1,      0,         0,
               1,        0,      0,         L3,
               0,        0,      0,         1;
        
            
      R2_3<<  cos(theta3),          -sin(theta3),      0 ,          0,
              sin(theta3),           cos(theta3),      0,           0,
                0,                    0,            1,           0,
                0,                    0,            0,           1;
            
    
      p3_3 (1) = -H;
      p3_3 (3) = 1;

      
      p3_0 = T0_1 * R0_1 *T1_2 * R1_2*T2_3 * R2_3 * p3_3 ;

      
      //击球平面中心相对于原点的x,y,z坐标
      policy_ball.p3_0_x = p3_0(0);
      policy_ball.p3_0_y = p3_0(1);
      policy_ball.p3_0_z = p3_0(2);

      cout<<p3_0<<endl;
}



int main(int argc, char ** argv)
{

  PID_init();//pid初始化

  rclcpp::init(argc,argv);

  auto node = std::make_shared<HitCtrl>();
  rclcpp::spin(node);

  rclcpp::shutdown();

  return 0;
}




